#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include "string.h"
//#include <sys/ioctl.h>
#include <sys/mman.h>
#include <linux/videodev2.h>
#include <libv4l2.h>
#include "time.h"

#define CAM_DEVICE "/dev/video0"  // USB camera 
#define WIDTH 1920                //  
#define HEIGHT 1080
#define OUTPUT_FILE "frame.jpg"
#define FRAME_NUM  4
#define DESIRED_IMAGE_COUNT 4
#define INITIAL_IMAGE_PATH "/mnt/nfs/"
#define PIX_FORMAT V4L2_PIX_FMT_MJPEG //V4L2_PIX_FMT_YUYV  // YUYV or MJPEG

//#define printf(s) do{    \
//        fprintf(stderr,"[%s:%d] %s:%s\n",__FILE__,__LINE__,s,strerror(errno));\
//        exit(-1);\
//}while(0)
typedef struct VideoBuffer
{
    void   *start;
    size_t  length;
}VideoBuffer;

//get each image's path name via different catching time
void get_image_name(char _str[],int _num)
{
	char time_str[30];
	time_t timep;
	struct tm *p;
	time(&timep);
	p=localtime(&timep);
	
	sprintf(time_str,"%d%d%d_%d_%d_%d_%d",(1900+p->tm_year), (1+p->tm_mon),p->tm_mday, p->tm_hour, p->tm_min, p->tm_sec,_num);
	strcat(time_str,".jpg");
	
	strcat(_str,time_str);	
}
int main() {
    int fd = -1;
    char image_path[80]=INITIAL_IMAGE_PATH;
    // 1. open usb camera 
    fd = v4l2_open(CAM_DEVICE, O_RDWR);
    if (fd < 0) {
        printf ("Failed to open device\n");
        return -1;
    }
// 2. query device capability 
    struct v4l2_capability cap;
    if (v4l2_ioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) {
        printf("Query capabilities failed\n");
        goto error;
    }
    printf("========= Capability Informations: ==========\n");
    printf(" driver: %s\n", cap.driver);
    printf(" card: %s\n", cap.card);
    printf(" bus_info: %s\n", cap.bus_info);
    printf(" version: %08X\n", cap.version);
    printf(" capabilities: %08X\n", cap.capabilities);
    printf("=============================================\n");
    
    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
        fprintf(stderr, "Device does not support video capture\n");
        goto error;
    }
    //get camera support format
    struct v4l2_fmtdesc fmtdesc;
    fmtdesc.index = 0; //frame number
    fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;//frame type  
    printf("========= Video Format Informations: ==========\n");
    while(v4l2_ioctl(fd,VIDIOC_ENUM_FMT,&fmtdesc) != -1){  
        printf("VIDIOC_ENUM_FMT pixelformat:%s\n",fmtdesc.description);
    	fmtdesc.index++;
    }
    printf("=========== Video Size Informations: ==========\n");
    //get camera support size
    struct v4l2_frmsizeenum frmsize;
    frmsize.pixel_format = PIX_FORMAT;  // set pixel format
    frmsize.index = 0;
    while (v4l2_ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frmsize) == 0) {
        if (frmsize.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
        	printf("Supported resolution: %dx%d\n", 
            	frmsize.discrete.width, frmsize.discrete.height);
        }
    	frmsize.index++;
    }
    printf("===============================================\n");

    // 3. set format 
    struct v4l2_format fmt;
    memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = WIDTH;
    fmt.fmt.pix.height = HEIGHT;
    fmt.fmt.pix.pixelformat = PIX_FORMAT;
    fmt.fmt.pix.field = V4L2_FIELD_NONE;
    if (v4l2_ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
        printf("Set format failed\n");
        goto error;
    }
    if (v4l2_ioctl(fd, VIDIOC_G_FMT, &fmt) < 0) {
	    printf("ioctl failed\n");
    }
    printf("=========== Stream Format Informations: ========\n");
    printf(" type: %d\n", fmt.type);
    printf(" width: %d\n", fmt.fmt.pix.width);
    printf(" height: %d\n", fmt.fmt.pix.height);

    // 4. request buf in device 
    struct v4l2_requestbuffers req;
    memset(&req, 0, sizeof(req));
    req.count = FRAME_NUM;  // frame capture number 
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;
    if (v4l2_ioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
        printf("Request buffers failed\n");
        goto error;
    }

    // 5. MMAP( device buf map to user buf) 
    int i;
    //void *buffers[FRAME_NUM];
    VideoBuffer* video_buf = calloc(req.count, sizeof(*video_buf));
    struct v4l2_buffer buf;
    memset(&buf, 0, sizeof(buf));
    for (i=0; i< req.count; i++) { 
        //printf("------ i=%d------\n", i);
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;
        if (v4l2_ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) {
            printf("Query buffer failed\n");
            goto error;
        }
        video_buf[i].length = buf.length;
        video_buf[i].start = v4l2_mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
        if (video_buf[i].start == MAP_FAILED) {
            printf("mmap failed\n");
            goto error;
        }
        //  queue buffer to save images from device
        if (v4l2_ioctl(fd, VIDIOC_QBUF, &buf) < 0) {
            printf("Queue buffer failed\n");
            goto error;
        }
        printf("Frame buffer %d: address=0x%x, length=%d\n", i, video_buf[i].start, video_buf[i].length);
    }
    printf("------ buf.index=%d------\n", buf.index);
    // 6. start stream
    if (v4l2_ioctl(fd, VIDIOC_STREAMON, &buf.type) < 0) {
        printf("Start stream failed\n");
        goto error;
    }

    // 7. capture one frame data and save file
    int loop_i;
    for(loop_i=0; loop_i < DESIRED_IMAGE_COUNT; loop_i++ ) {
        //get frame data
        if (v4l2_ioctl(fd, VIDIOC_DQBUF, &buf) < 0) {
            printf("VIDIOC_DQBUF failed\n");
            goto error;
        }
        //save file
        
        strcpy(image_path,INITIAL_IMAGE_PATH);//Re-initialise  path name of image
        get_image_name(image_path,loop_i+1);//get new path name
        FILE *fp = fopen(image_path, "wb");
        if (fp < 0) 
		{
			printf("open frame data file failed\n");
			exit(0);
		}
        fwrite(video_buf[buf.index].start, 1, buf.length, fp);
        fclose(fp);
        printf("Captured frame%d save in %s\n", loop_i+1, image_path);

        // Re-queen buffer
		if (v4l2_ioctl(fd, VIDIOC_QBUF, &buf) < 0)
		{
			printf("VIDIOC_QBUF failed \n");
			goto error;
		}
    }
    //FILE *fp = fopen(OUTPUT_FILE, "wb");
    //fwrite(buffers[FRAME_NUM-1], buf.bytesused, 1, fp);
    //fclose(fp);
    //printf("Captured %d bytes to %s\n", buf.bytesused, OUTPUT_FILE);
    
    // 9. release
    v4l2_ioctl(fd, VIDIOC_STREAMOFF, &buf.type);
    for (i=0; i<req.count; i++){
        v4l2_munmap(video_buf[i].start, video_buf[i].length);
    }
//close file of camera
	printf("\n#close camera:\n");
	if(fd != -1)
	{			
		if(v4l2_close(fd) != -1)
			printf("succeed to close camera!\n");
	}
	else
	{
		printf("failed to close camera!\n");
		exit(0);
	}
 
//free memory 
    free(video_buf);
error:
    v4l2_close(fd); 
    return -1;
}
